Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add API to gracefully cancel a controller #4136

Conversation

Rayman
Copy link
Contributor

@Rayman Rayman commented Feb 22, 2024


Basic Info

Info Please fill out this column
Ticket(s) this addresses #4033
Primary OS tested on Ubuntu
Robotic platform tested on Gazebo simulation of inhouse robot
Does this PR contain AI generated software? No

Description of contribution in a few bullet points

Ability to let controllers handle cancelations gracefully. See the discussion at #4033.

I'm opening this PR for a first review of the API. I've added gracefull cancellation to one of our in-house controllers and tested that this API is sufficient. I hope the API fits the design of the core APIs.

Description of documentation updates required from your changes


Future work that may be required in bullet points

For Maintainers:

  • Check that any new parameters added are updated in navigation.ros.org
  • Check that any significant change is added to the migration guide
  • Check that any new features OR changes to existing behaviors are reflected in the tuning guide
  • Check that any new functions have Doxygen added
  • Check that any new features have test coverage
  • Check that any new plugins is added to the plugins page
  • If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

@Rayman Rayman force-pushed the feature/4033/add-graceful-cancellation-api branch from 87c468f to 52327c8 Compare February 22, 2024 14:17
@Rayman Rayman marked this pull request as draft February 22, 2024 14:17
@Rayman Rayman force-pushed the feature/4033/add-graceful-cancellation-api branch from 52327c8 to 1d22c51 Compare February 22, 2024 15:00
Copy link

codecov bot commented Feb 22, 2024

Codecov Report

Attention: Patch coverage is 75.00000% with 2 lines in your changes are missing coverage. Please review.

Project coverage is 89.86%. Comparing base (80bb5bf) to head (1d22c51).

❗ Current head 1d22c51 differs from pull request most recent head be2e9d2. Consider uploading reports for the commit be2e9d2 to get more accurate results

Files Patch % Lines
nav2_controller/src/controller_server.cpp 66.66% 2 Missing ⚠️
Additional details and impacted files
@@            Coverage Diff             @@
##             main    #4136      +/-   ##
==========================================
+ Coverage   89.49%   89.86%   +0.37%     
==========================================
  Files         431      432       +1     
  Lines       19555    19411     -144     
==========================================
- Hits        17500    17443      -57     
+ Misses       2055     1968      -87     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

@SteveMacenski
Copy link
Member

SteveMacenski commented Feb 22, 2024

I think having the cancel state projected back to the computeVelocityCommands as an argument would be important. Right now, I don't know how the controller would know when cancel stopped and a new request came in otherwise. So I think cancelling should be given to it.

Or possibly even have separate computeCancelCommands() API to use when cancelling = true. That may be a more ergonomics thing: I imagine that when you have a controller that is handling normal vs cancel state, you're basically having an if/else for 2 very different methods either way. Those would probably render into separate functions, so we could just call separate functions. We could have the if/else in computeAndPublishVelocity with new arg cancelling

Then, the if (controllers_[current_controller_]->cancel()) { check woudl be more like cancelled() to indicate that its completed its cancellation to allow us to action_server_->terminate_all(); and publishZeroVelocity(); for good measure (and where the timeout is located)

So all in all, I like your design, but I think we an do it a little bit better with addresses the problem of the controller knowing when we're back outside of the non-canceled state for normal operations & adding a timeout in case a plugin isn't properly implemented

@Rayman
Copy link
Contributor Author

Rayman commented Feb 23, 2024

@SteveMacenski which controller is the easiest to modify with this new API? I was thinking about the pure pursuit controller but that one doesn't have decelleration limits so I don't know how fast to brake. How is the acceleration determined?

@SteveMacenski
Copy link
Member

Which ever one you like - I think DWB is the only one with explicit decel/accel parameters right now. MPPI will have it before EOY, but not its current state. RPP is a pure geometric approach, so there's not really any dynamic modeling (right, its just following a carrot). We have some constraints on speed based on some heuristic functions, but that's also not dynamics based, its situational.

@Rayman Rayman force-pushed the feature/4033/add-graceful-cancellation-api branch from 1d22c51 to 93f08e9 Compare March 14, 2024 09:43
Copy link
Contributor

mergify bot commented Mar 14, 2024

@Rayman, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@SteveMacenski
Copy link
Member

@Rayman anything I need to review? Or still working on it 😄

@Rayman Rayman force-pushed the feature/4033/add-graceful-cancellation-api branch from 93f08e9 to 197eb53 Compare March 19, 2024 15:23
Copy link
Contributor

mergify bot commented Mar 19, 2024

@Rayman, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@Rayman
Copy link
Contributor Author

Rayman commented Mar 19, 2024

Yes sorry I'm still working on it.

I've modified the RPP controller to add a cancel_decelleration parameter that shows how to use the new API. Is this in line with the design of the controller? I thought that one was the easiest to modify. Most controllers don't have acceleration limits, only the DWB. But that one is quite complicated with the critics so I didn't know how to modify it to support cancellation.

I've also adressed one of your review comments. Last thing to do is add some unit tests probably.

image

@Rayman Rayman force-pushed the feature/4033/add-graceful-cancellation-api branch from 197eb53 to d5571c9 Compare March 19, 2024 15:30
Copy link
Contributor

mergify bot commented Mar 19, 2024

@Rayman, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@SteveMacenski
Copy link
Member

Generally looks good to me!

@Rayman Rayman force-pushed the feature/4033/add-graceful-cancellation-api branch from d5571c9 to a80350f Compare March 26, 2024 10:46
Copy link
Contributor

mergify bot commented Mar 26, 2024

@Rayman, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@Rayman Rayman marked this pull request as ready for review March 26, 2024 12:18
@Rayman
Copy link
Contributor Author

Rayman commented Mar 26, 2024

I don't understand the CI failure. I didn't edit any package.xml so why is it complaining about dependency issues?

@Rayman Rayman changed the title WIP: Add API to gracefully cancel a controller Add API to gracefully cancel a controller Mar 26, 2024
@SteveMacenski
Copy link
Member

SteveMacenski commented Mar 27, 2024

@Rayman we're in a weird state due to the rolling transition to 24.04. I'll plan to look into it in more detail on Friday, but for now, if you tell me it works and you tested it, I'd trust that assuming the other checks look good

Copy link
Contributor

mergify bot commented Mar 28, 2024

@Rayman, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@Rayman
Copy link
Contributor Author

Rayman commented Mar 28, 2024

I've tested this only in simulation for now. I'll test this on a robot in the next coming weeks but I need some more parts to be ready for that. I'll send this PR also to a collegue to review

Copy link
Contributor

@Timple Timple left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looks good to me!

This now works in simulation for the pure pursuit and our own controller. So for me that's good enough 🙂. I don't think the physical interfaces of the robot will influence this behavior. But I'll leave that up to @SteveMacenski

RCLCPP_INFO_THROTTLE(
get_logger(),
*get_clock(), 1000,
"Goal was canceled. Requesting controller to stop robot gracefully.");
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nitpick: this is an implementation detail. "Waiting for the controller to finish cancellation"

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

On the other hand, publishZeroVelocity(); enforces a stop, so this is the only option 😄. You can leave it.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure, @Rayman can you make that string update? Then I can merge

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done

@SteveMacenski
Copy link
Member

SteveMacenski commented Mar 28, 2024

Sim is good enough for me - yal know what you're doing. Feel free to implement other controllers if you like (or contribute your controller here) 😉

@Rayman Rayman force-pushed the feature/4033/add-graceful-cancellation-api branch from b07b0a3 to be2e9d2 Compare March 29, 2024 09:59
@SteveMacenski SteveMacenski merged commit 2ecc91e into ros-navigation:main Mar 29, 2024
9 of 11 checks passed
@SteveMacenski
Copy link
Member

Thanks @Rayman !

@Rayman Rayman deleted the feature/4033/add-graceful-cancellation-api branch April 2, 2024 11:42
@doisyg
Copy link
Contributor

doisyg commented Apr 4, 2024

Nice work, I haven't tested it but reading on the changes, 2 questions:

  1. In the way it is implemented in RPP, I believe it is now a mandatory behavior to use the cancel_deceleration. Any way to making it optional, for instance when one wants to rely on the deceleration configured and applied in a velocity smoother node (and without having to manage another decel limit parameter somewhere else?
  2. Is it applied when preempting the controller server action ? (in my current use-case I would not want that, but I can think of use cases where it would be desirable)

@SteveMacenski
Copy link
Member

SteveMacenski commented Apr 4, 2024

I believe it is now a mandatory behavior to use the cancel_deceleration

You could set the deceleration to something insanely high to in effect disable, but this is a good point, it should be an option. We can make a param pretty easily that'll bypass this if needed. I filed a ticket #4238 for record keeping

Is it applied when preempting the controller server action

Nope, only on cancel:

      if (action_server_->is_cancel_requested()) {
        if (controllers_[current_controller_]->cancel()) {
          ...
        } ...
      }

@doisyg
Copy link
Contributor

doisyg commented Apr 4, 2024

All good short term then!

Timple pushed a commit to nobleo/navigation2 that referenced this pull request Apr 10, 2024
* Add API to gracefully cancel a controller

Signed-off-by: Ramon Wijnands <[email protected]>

* Add `cancel_deceleration` to RegulatedPurePursuitController

Signed-off-by: Ramon Wijnands <[email protected]>

* Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>

---------

Signed-off-by: Ramon Wijnands <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
ajtudela pushed a commit to grupo-avispa/navigation2 that referenced this pull request Apr 19, 2024
* Add API to gracefully cancel a controller

Signed-off-by: Ramon Wijnands <[email protected]>

* Add `cancel_deceleration` to RegulatedPurePursuitController

Signed-off-by: Ramon Wijnands <[email protected]>

* Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>

---------

Signed-off-by: Ramon Wijnands <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
enricosutera pushed a commit to enricosutera/navigation2 that referenced this pull request May 19, 2024
* Add API to gracefully cancel a controller

Signed-off-by: Ramon Wijnands <[email protected]>

* Add `cancel_deceleration` to RegulatedPurePursuitController

Signed-off-by: Ramon Wijnands <[email protected]>

* Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>

---------

Signed-off-by: Ramon Wijnands <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: enricosutera <[email protected]>
Marc-Morcos pushed a commit to Marc-Morcos/navigation2 that referenced this pull request Jul 4, 2024
* Add API to gracefully cancel a controller

Signed-off-by: Ramon Wijnands <[email protected]>

* Add `cancel_deceleration` to RegulatedPurePursuitController

Signed-off-by: Ramon Wijnands <[email protected]>

* Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>

---------

Signed-off-by: Ramon Wijnands <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
Manos-G pushed a commit to Manos-G/navigation2 that referenced this pull request Aug 1, 2024
* Add API to gracefully cancel a controller

Signed-off-by: Ramon Wijnands <[email protected]>

* Add `cancel_deceleration` to RegulatedPurePursuitController

Signed-off-by: Ramon Wijnands <[email protected]>

* Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>

---------

Signed-off-by: Ramon Wijnands <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
masf7g pushed a commit to quasi-robotics/navigation2 that referenced this pull request Oct 23, 2024
* Add API to gracefully cancel a controller

Signed-off-by: Ramon Wijnands <[email protected]>

* Add `cancel_deceleration` to RegulatedPurePursuitController

Signed-off-by: Ramon Wijnands <[email protected]>

* Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>

---------

Signed-off-by: Ramon Wijnands <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants